geometry_msgs/Pose stop_pose
float64 dist_to_stop_pose
geometry_msgs/Point[] stop_factor_points
